/**
 * @file recoder_pack.cpp
 * @brief 
 * @author haibo.yang
 * @version 1.0
 * @date 2024-02-28
 * 
 * @copyright Copyright (c) 2023  lgim
 * 
 */
#include "record_pack.h"
#include <dirent.h>
#include <errno.h>
#include <sys/stat.h>
#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <fstream>  
#include <iostream>  
#include <sys/syscall.h>
#include <sys/resource.h>  
#include <ctime>  
#include <agv_msgs/event_codes.h>
#include "canlog.h"
#include "shared_parameters/shared_params_interface.hpp"

namespace record {

#define BASEDIR std::string(getenv("HOME"))
#define BLACKBOXDIR BASEDIR + "/record"
#define LOGDIR BASEDIR + "/log"
#define TEMLOG "/tmp/log"

std::vector<PackAge> pakeages;

RecordPack::RecordPack(const double interval): 
    rclcpp::Node("record"),
    // path_(""),
    interval_(interval),
    shutdown_req_(false),
    mask_auto_trigger_(false),
    zipper_(nullptr),
    // file_count_(0),
    max_package_count_(10)                                                            
{
    init_parameters();
}

RecordPack::~RecordPack()
{
    if (zipper_ != nullptr) {
        zipper_->close();
        delete zipper_;
        zipper_ = nullptr;
    }

    if (thread_.joinable()) {
        thread_.join();
    }

    if (candump_thread_.joinable()) {
        stop();
        candump_thread_.join();
    }
}

void RecordPack::CheckLogPath(void) 
{   
    std::string path = LOGDIR;
    if (access(path.c_str(), 0) == -1) { 
        int flag = mkdir(path.c_str(), S_IRWXU);  
        if (flag == 0) {  
            INFO("Create directory " << path << " successfully.");
        } else { 
            ERROR("Fail to create " << path << "directory " );
            throw std::exception();
        }
    } else {
   	    INFO(path << " already exists.");
    }

    path = BLACKBOXDIR;
    if (access(path.c_str(), 0) == -1) { 
        int flag = mkdir(path.c_str(), S_IRWXU);  
        if (flag == 0) {  
            INFO("Create directory " << path << " successfully.");
        } else { 
            ERROR("Fail to create " << path << "directory " );
            throw std::exception();
        }
    } else {
   	    INFO(path << " already exists.");
        SearchDir(false, BLACKBOXDIR, COLLECT);

        std::sort(pakeages.begin(), pakeages.end(), [](const std::pair<std::string, std::time_t>& a,  
                                           const std::pair<std::string, std::time_t>& b) {  
            return a.second < b.second; 
        });  
    
        for (const auto& pair : pakeages) {  
            INFO(pair.first << " at " << pair.second);  
            blackbox_.package_queue.push(pair);
        }  

        CheckPackage();
    }
}

bool RecordPack::init_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    INFO( "Failed to initialize shared memory interface");
    return false;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      INFO("Error get shared parameters");
      return false;
    } 

    params = SharedParamsInterface::getNodeParams("Record");
    for (const auto& [name, value] : params) {
      INFO("get param:" << name << ", value:" << value.toString());
      switch(value.type) {
        case SharedParamsInterface::ParamValue::Type::INT:
            declare_parameter(name, value.getValue<int>());
            break;
        case SharedParamsInterface::ParamValue::Type::DOUBLE:
            declare_parameter(name, value.getValue<double>());
            break;
        case SharedParamsInterface::ParamValue::Type::STRING:
            declare_parameter(name, value.getValue<std::string>());
            break;
        case SharedParamsInterface::ParamValue::Type::BOOL:
            declare_parameter(name, value.getValue<bool>());
            break;
        default:
            WARN("Unsupported parameter type for " << name);
            break;
      }
    }
  } catch (const std::exception& e) {
    ERROR("Error reading parameters: " << e.what());
    return false;
  }
  return true;
}


void RecordPack::Init()
{ 
    INFO("init.");
    int size;
    std::string names;

    // Blackbox maximum quantity
    get_parameter_or("MaxCount", max_package_count_, 20);
    get_parameter_or("CanLogSize", size, 10);
    get_parameter_or("CanLogNames", names, std::string("can0,can1"));
    get_parameter_or("MaskAutoTrigger", mask_auto_trigger_, false);
    get_parameter_or("AutoTriggerFiles", auto_trigger_files_, 2);
    get_parameter_or("EstopManualTrigger", estop_manual_trigger_, false);

    if (max_package_count_ > 100) max_package_count_ = 100;
    else if (max_package_count_ < 0) max_package_count_ = 10;

    if (size > 500) size = 500; else if (size < 0) size = 10;

    if (auto_trigger_files_ > 10) auto_trigger_files_ = 10; 
    else if (auto_trigger_files_ <= 0) auto_trigger_files_ = 2;
       
    INFO("Maximum number of black boxes:" << max_package_count_);
    INFO("CanLogSize:" << size);
    INFO("Can log device:" << names);
    INFO("Mask Auto trigger:" << mask_auto_trigger_);
    INFO("Auto trigger files:" << auto_trigger_files_);

    CheckLogPath();

    pack_server_ = create_service<std_srvs::srv::SetBool>(
        "/compress_data", 
        std::bind(&RecordPack::PackLogCallback, this, std::placeholders::_1, std::placeholders::_2));
    
    event_sub_ = create_subscription<agv_msgs::msg::AGVEventStatus>(
        "/eventstatus", 10, std::bind(&RecordPack::EventCallBack, this, std::placeholders::_1));

    std::string can_names = names; 
    size_t buffer_size = size * 1024 * 1024;
    // start candump
    candump_thread_ = boost::thread([can_names, buffer_size]() {
        canlog(can_names.c_str(), buffer_size);
    });

    // start thread
    thread_ = boost::thread(boost::bind(&RecordPack::Run, this));
}

std::vector<LOGFile> RecordPack::ListFiles(const bool autotype, const fs::path& dir, const std::string& prefix) 
{  
    std::vector<LOGFile> files; 

    if (!fs::exists(dir)) {
        ERROR("Directory does not exist: " << dir);
        return files;
    } 
    try {
        for (const auto& entry : fs::directory_iterator(dir)) {
            const fs::path& filePath = entry.path();
            const std::string entryName = prefix + filePath.filename().string();
            
            if (fs::is_directory(filePath)) {
                std::vector<LOGFile> subfiles = ListFiles(false, filePath, entryName + "/");  // Recursive call for subdirectories
                files.insert(files.end(), subfiles.begin(), subfiles.end());
            } else {
                if (autotype) {
                    size_t dotPos = entryName.rfind('.');  
                    if (dotPos != std::string::npos &&  
                        dotPos != entryName.size() - 1 && 
                        entryName.substr(dotPos + 1) == "log") {  
                        files.push_back(std::make_pair(entryName, entry.path().string()));
                        continue;
                    }  

                    for (int i=1;i<auto_trigger_files_;i++) {
                        std::string file_suffix = "." + std::to_string(i) + ".log";  
                        size_t found_pos = entryName.find(file_suffix);  
                        if (found_pos != std::string::npos) {  
                            files.push_back(std::make_pair(entryName, entry.path().string()));
                        }
                    }
                } else {
                    files.push_back(std::make_pair(entryName, entry.path().string()));
                }
            }
        }
    } catch (const std::filesystem::filesystem_error& e) {
       ERROR("Filesystem error: " << e.what());
    } catch (const std::exception& e) {
       ERROR("Error: " << e.what());
    }
    return files;  
}  

void RecordPack::CleanUpExpired(const bool autotype)
{
    for (const auto& entry : fs::directory_iterator(LOGDIR)) {
        if (!fs::is_directory(entry)) {
            const fs::path& filePath = entry.path();
            const std::string fileName = filePath.filename().string();
            // INFO("filePath:" << filePath << ", fileName:" << fileName);
            DeleteAndCleanLogFiles(fileName, filePath.string(), autotype);
            INFO("Clean file: " << entry.path());
        } else {
            // log/ros
            // DeleteAllExceptSymbolicLinkDir(entry.path());
            try {
                INFO("Deleted directory:" << entry);
                fs::remove_all(entry.path());
            } catch (const std::exception& e) {
                ERROR("Delete error: " << e.what());
            }
        }
    }
}


void RecordPack::DeleteAllExceptSymbolicLinkDir(const fs::path& path) 
{
    // std::pair<fs::path, fs::path> latest;
    // for (const auto& entry : fs::directory_iterator(path)) {
    //     if (fs::is_symlink(entry.path())) {
    //         latest.first = entry.path();
    //         latest.second = fs::read_symlink(entry);
    //         INFO("Find " << latest.first << " -> " << latest.second);
    //         break;
    //     }
    // }

    for (const auto& entry : fs::directory_iterator(path)) {
        if (fs::is_directory(entry)) {
            fs::path currentPath = entry.path();
            // if (!fs::equivalent(fs::canonical(currentPath), fs::canonical(latest.first)) && 
            //     !fs::equivalent(fs::canonical(currentPath), fs::canonical(latest.second))) {
                fs::remove_all(currentPath);
                INFO("Deleted directory: " << currentPath);
            // }
        } else {
            fs::remove(entry);
            INFO("Deleted file: " << entry.path());
        }
    }

    // for (const auto& entry : fs::directory_iterator(latest.second)) {
    //     if (!fs::is_directory(entry)) {
    //         Clear(entry.path().string());
    //         INFO("Clean file: " << entry.path());
    //     }
    // }
}

int RecordPack::SearchDir(const bool autotype, const std::string path, const int code)
{
    int ret = 0; 
    if (!fs::exists(path) || !fs::is_directory(path)) {
        ERROR(" Path error: " << path);
        return -1;
    }

    INFO("File Path : " << path);  
    std::vector<LOGFile> files = ListFiles(autotype, path); 

    ret = files.size();
    for (const auto& file : files) {  
        
        if (code == COMPRESS) {
            // compress files to blackbox
            ASSERT(zipper_ != nullptr);
            if (zipper_->add(std::string(file.first), file.second) < 0) {
                ret = -1;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
        } else if (code == COLLECT) {
            // get blackbox list 
            struct stat filestat;  
            size_t dot_pos = file.second.rfind('.');  

            if (dot_pos == std::string::npos) {  
                continue;
            }  

            if (file.second.substr(dot_pos + 1) != "zip") {
                continue;
            }

            if (stat(file.second.c_str(), &filestat) == 0) {  
                std::time_t ctime = filestat.st_mtime;

                pakeages.push_back(std::make_pair(file.second, ctime));
                INFO("File modification time: " << ctime);  
            } else {  
                ERROR("Error getting file status: " << strerror(errno));  
                ret = -1;  
            }  
        }        
    }  
    
    return ret;
}

bool RecordPack::PackLogCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request> req, 
            std::shared_ptr<std_srvs::srv::SetBool::Response> resp)
{
    // if (!req.data) {
    //     resp.success = false;
    //     WARN(" req data error: " << req.data);
    //     return false;
    // }

    rclcpp::Time now = this->now();
    INFO("now:" << now.seconds() << ", last package time:" << blackbox_.package_time.seconds());
    if (blackbox_.state == BlackBox::State::idle && now.seconds() - blackbox_.package_time.seconds() > 10) {
        // Start thread to handle the record package asynchronously
        future_ = std::async(std::launch::async, [this, now]() {
            Package("MANUAL", now); // manual blackbox
        });
    } else {
        resp->success = false;
        WARN("The two packing times are less than 10s: " << (now - blackbox_.package_time).seconds());
        return false;
    }

    resp->success = true;
    resp->message = "Package successfully.";
    return true;
}

void RecordPack::Package(const std::string name, const rclcpp::Time current, const int eventcode) 
{
    std::lock_guard<std::mutex> lock(mutex_);
    blackbox_.state = BlackBox::State::packing;
    
    INFO("thread start :" << boost::this_thread::get_id());

    time_t now = time(nullptr);
    tm*  local_time = localtime(&now);
    char  file_suffix[256];
    
    bool autotype = true;
    size_t found = name.find("AT");  
  
    if (found == std::string::npos) 
        autotype = false;

    // delay 1 seconds 
    std::this_thread::sleep_for(std::chrono::seconds(1));
    
    sprintf(file_suffix, "#%d_%04d%02d%02d_%02d%02d%02d.zip", eventcode,
                                                        local_time->tm_year + 1900, 
                                                        local_time->tm_mon + 1, 
                                                        local_time->tm_mday,  
                                                        local_time->tm_hour,  
                                                        local_time->tm_min,
                                                        local_time->tm_sec);

    EventCodes codes;
    std::map<int, std::string> ecodes = codes.getEventCodes();

    std::string target_file_name = (autotype ? ecodes[eventcode] : name) + file_suffix;
    std::string target_path = BLACKBOXDIR + "/" + target_file_name;

    CopyFolder(LOGDIR, TEMLOG);
    CleanUpExpired(autotype);

    INFO("create blackbox: " << target_path);
    
    zipper_ = new Zipper(target_path);
    size_t cout = SearchDir(autotype, TEMLOG);

    INFO("A total of " << cout << " files were packed");

    zipper_->close();
    delete zipper_;
    zipper_ = nullptr;

    // delete log folder
    DeleteDirectory(TEMLOG);
     
    blackbox_.package_event = eventcode;
    blackbox_.package_time = current;
    blackbox_.package_queue.push(std::make_pair(target_path, now));
    blackbox_.state = BlackBox::State::idle;

    INFO("thread end");
}

int RecordPack::DeleteAndCleanLogFiles(const std::string fileName, const std::string filePath, const bool autotype)
{
    bool need_to_delete = false;
    bool need_to_clear = true;
    for (int i=1;i<21;i++) {
        std::string file_suffix = "." + std::to_string(i) + ".log";  
        size_t found_pos = fileName.find(file_suffix);  
        if (found_pos != std::string::npos) {  
            if (!autotype || (autotype && i < auto_trigger_files_)) {
                need_to_delete = true;
                INFO("file need to deleted : " << fileName);  
            } else if (autotype) {
                need_to_clear = false;
            }
        } 
    }

    if (need_to_delete) {
        // Delete it anyway
        Remove(filePath);
    } else if (need_to_clear) {
        // Clear file contents
        Clear(filePath);
    }
    return 0;
}

void RecordPack::Run()
{
    INFO("thread id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));
    while (rclcpp::ok() && !shutdown_req_) {
        {
            std::lock_guard<std::mutex> lock(mutex_);
            CheckPackage();
        }
        // Sleep for 1 milliseconds
        boost::this_thread::sleep_for(boost::chrono::milliseconds(int(interval_* 1000)));
    }
    INFO("exists.");
}

void RecordPack::EventCallBack(const agv_msgs::msg::AGVEventStatus::SharedPtr event)
{
    events_ = *event;
    rclcpp::Time timestamp = this->now();
    bool manual_mode = false;

    if (mask_auto_trigger_) {
        INFO("Mask auto trigger record bag.");
        return;
    }

    // 2025.4.9 event package
    if (events_.ecode == agv_msgs::msg::AGVEvent::NONE_EVENT) {
        if (blackbox_.package_event > 0)
            blackbox_.package_event = -1;
        return;
    } else if (events_.type != agv_msgs::msg::AGVEvent::ALARM && 
        events_.action != agv_msgs::msg::AGVEvent::ESTOP) {
        return;
    } else if(events_.ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT) {  
        if (!estop_manual_trigger_)
            return;
        else
            manual_mode = true;
    } else if (events_.ecode == agv_msgs::msg::AGVEvent::BUMPER_EVENT || 
        events_.ecode == agv_msgs::msg::AGVEvent::SAFE_DETECT_NEAR_EVENT) {
        return;
    } 
        
    INFO(
        "Trigger record bag (" << (manual_mode ? "manaul" : "auto") << ")" <<
        "new event code:" << events_.ecode << 
        " type:" << events_.type << 
        " action:" << events_.action << 
        " package event:" << blackbox_.package_event);

    if (blackbox_.state == BlackBox::State::idle) {    
        try {
            auto time_diff = timestamp - blackbox_.package_time;
            
            if (time_diff > rclcpp::Duration(10, 0)) {
                if (blackbox_.package_event != events_.ecode) {
                    INFO("Record package event:" << events_.ecode);
                    // Start thread to handle the record package asynchronously
                    future_ = std::async(std::launch::async, [this, timestamp, manual_mode]() {
                        if (!manual_mode)
                            Package("AT", timestamp, events_.ecode); // Auto package blackbox
                        else
                            Package("MANUAL", timestamp, events_.ecode); // Auto package blackbox
                    });
                }
            } else {
                INFO("The two packing times are less than 10s: " << time_diff.seconds());
            }
        } catch (const std::runtime_error& e) {
            ERROR("Time subtraction failed: " << e.what());
            blackbox_.package_time = timestamp - rclcpp::Duration(11, 0);
            INFO("Reset blackbox_.package_time to current timestamp due to clock mismatch");
        } catch (...) {
            ERROR("Unknown error during time calculation");
        }
    }
}

};

// 2024.8.28 add cpu affinity used
void set_cpu_affinity(int cpu_id) {  
    cpu_set_t cpuset;  
    pthread_t thread = pthread_self();  
    CPU_ZERO(&cpuset);  
    CPU_SET(cpu_id, &cpuset);  
  
    if (pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset) != 0) {  
        std::cout << "Error setting CPU affinity" << std::endl;  
    } else {  
        std::cout << "Thread bound to CPU " << cpu_id << std::endl;  
    }  
}  

int main(int argc, char *argv[])
{
    set_cpu_affinity(CPU_ID); 
    // set nice value
    if (setpriority(PRIO_PROCESS, getpid(), 10) == -1) {  
        perror("setpriority");  
    }  
  
    rclcpp::init(argc, argv);
    auto node = std::make_shared<record::RecordPack>(0.1);
    node->Init();
   
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}
